#include "widget.h"
#include "config.h"

Widget::Widget(QWidget *parent): QWidget(parent)
{
    Init_Mainform();
    Init_Tab1();
    Init_Tab2();
    Init_Tab3();
    Init_Tab4();
    Init_Tab5();
    Init_Tab6();
    datafile.Add_File(STRING(DATAFILE_PATH));
    Load_Control_data();
    for (int i=0; i<6; ++i)
        emit slid1Tuning1[i]->valueChanged(0);
    emit btn3Options[0]->clicked();
}

Widget::~Widget()
{
    if (ckbx3Options[1]->isChecked())
        Save_Control_data();
}

void Widget::Load_Control_data()
{
    double data;
    int i;
    for (i=0; i<12; i++){
        if (datafile.Load_Data(i, &data)) continue;
        if (i<6) ledt3Posture[i]->setText(QString::number(data, 'g', 8));
        else if (i<12) ledt3PostureDeltaValue[i-6]->setText(QString::number(data, 'g', 8));
    }
    if (datafile.Load_Data(i++, &ar3._Eff._x)) return;
    if (datafile.Load_Data(i++, &ar3._Eff._y)) return;
    if (datafile.Load_Data(i++, &ar3._Eff._z)) return;
}

void Widget::Save_Control_data()
{
    using namespace std;
    double data;
    string s;
    int i;
    datafile.File_Clear();
    for (i=0; i<12; i++){
        if (i<6) s = ledt3Posture[i]->text().toStdString();
        else if (i<12) s = ledt3PostureDeltaValue[i-6]->text().toStdString();
        if (s=="") continue;
        try {data = stod(s);}
        catch (exception e) {cout << e.what() << endl;}
        AR3PC_ASSERT_ERROR(!datafile.Save_Data(i, data), "Control data saving failed!");
    }
    AR3PC_ASSERT_ERROR(!datafile.Save_Data(i++, ar3._Eff._x), "Control data saving failed!");
    AR3PC_ASSERT_ERROR(!datafile.Save_Data(i++, ar3._Eff._y), "Control data saving failed!");
    AR3PC_ASSERT_ERROR(!datafile.Save_Data(i++, ar3._Eff._z), "Control data saving failed!");
}

void Widget::Trajectory_Init()
{
    double distance=0;
    for(int i=0;i<3;++i)
        distance += (ar3._start[i] - ar3._end[i]) * (ar3._start[i] - ar3._end[i]);
    ar3._t = 0;
    ar3._totalT = sqrt(distance)/ar3._Vmove;
    ar3._isworking = true;
}

void Widget::Trajectory_Update()
{
    int pulse;
    double rate = ar3._t / ar3._totalT;
    double tempPosture[6], tempJointAngle[6];
    double roundtemp;
    for (int i=0; i<6; ++i)
        tempPosture[i] = (1-rate)*ar3._start[i] + rate*ar3._end[i];
    if(ar3.Inverse_Solve(tempPosture, tempJointAngle) != 0){
        MESSAGE_ERROR("逆解不存在!");
        return;
    }
    for (int i = 0; i < 6; ++i) {
        roundtemp = (tempJointAngle[i] - ar3.NegRadLim[i]) / ar3.DriveRatio[i];
        PulseTargetTab1[i] = ROUND(roundtemp);
        if(PulseTargetTab1[i] < 0)
            ckbx2Reverse[i]->setChecked(true);
        else
            ckbx2Reverse[i]->setChecked(false);
        pulse = ABS(PulseTargetTab1[i]);
        slid1Tuning1[i]->setValue(pulse/100);
        slid1Tuning2[i]->setValue((pulse%100)/10);
        slid1Tuning3[i]->setValue(pulse%10);
    }
    ar3._t += 0.1;
    if (ar3._t > ar3._totalT){
        //Slot_btn3SynchroniseStep_Click();
        ar3._isworking = false;
    }
}

void Widget::Security_Check()
{
    u8 danger = 0;
    double tempPosture[3];
    ar3.Forward_Solve(JointAngleTab1, tempPosture, ar3.JOINT6);
    if (tempPosture[0] > -27) danger |= (1<<0);
    if (tempPosture[2] < 155) danger |= (1<<1);
    if (danger){
        ckbxEmergency->setChecked(true);
        emit btnEmergencyStop->clicked();
        MESSAGE_ERROR("危险!");
    }
}
